-module(rover).
%%-export([]).
-compile(export_all).


go() ->
    start("127.0.0.1", 17676).


start(Host, Port) ->
    S = connect(Host, Port),

    P = spawn(fun() -> toserver(S) end),
%%%%    C = spawn(fun() -> controller([{sender, P}, {last_telem, void}, {toclient, self()}], initial()) end),
    
    L = erlang:open_port({spawn, "./high.bin"}, []),
						%    L = void,


    telemetry(S, void, L, P). % P -- ToServer


%% !!! From 0..+infinite counterclockwise to Martian 
compute_angle(A) ->
    A10 = A rem 360,
    if
	A10 > 180 ->
	    (A10 - 180) - 180;

	true ->
	    A10
    end.


initial() ->
%    turn_home(compute_angle(314)).
    nop().


%% -----------------


connect(H, P) ->
    {ok, S} = gen_tcp:connect(H, P, [list, {packet, 0}, {active, true}]),
    S.


toserver(S) ->
    receive
	M % when is_list(M) 
	->
	    gen_tcp:send(S, M),
	    toserver(S)

    end.


controller(M, {Decide, _, _}) ->
    receive
	stop -> 
	    ok;

	{goal, {Decid, _, _}=Goal} ->

	    NewM = process_goal(M, last_telem(M), Goal), % ???
	    NewG = Decid(NewM, last_telem(M)),

	    controller(NewM, NewG);       % unconditional change of the current goal

	{telem, T} ->
%% composition IS HERE!!!
	    G = Decide(M, T),
	    controller(process_goal(M, T, G), G)
    end.



process_goal(M, T, {_, UpdateM, SendMessages}) ->
    SendMessages(M, T),
    UpdateM(M, T).


telemetry(S, Pid, Port, ToServer) ->
    receive
	don ->
	    io:format("DONE~n"),
	    Port ! {self(), {command, "D ; "}},
	    
	    telemetry(S, Pid, Port, ToServer);


	{tcp, S, L} ->

	    case Port of
		void ->
		    ok;
		_ ->
		    Port ! {self(), {command, L}}
	    end,	    


	    {Pid2} = parse(L, Pid, Port, ToServer),

	    
	    telemetry(S, Pid2, Port, ToServer);

% pipe
	{Port, {data, Data}} -> 

	    io:format("from pipe=~p.~n", [Data]),
%% from pipe="L 1 2 ; ".

	    [Data10|_] = string:tokens(Data, ";"),
	    {Data20, _} = read(string:tokens(Data10, " "), 2),

%%	    case Data of
%%		[$L|_] ->

		    io:format("pipe=~p.~n", [Data20]),
		    
		    Angle = hd(Data20),
%		    Speed = tl(Data20),
	    
		    Pid ! {goal, turn_home(
				   compute_angle(Angle)
				                   )},      % TODO speed !!!! 

%% 		[$R|_] ->
%% 		    io:format("Right.~n");

%% 		_ ->
%% 		    error
%% 	    end,


	    
	    telemetry(S, Pid, Port, ToServer)

    end.


parse([H|T]=LL, Pid, Port, ToServer) ->
    [M|_] = string:tokens(T, ";"),
    L = string:tokens(M, " "),
    case H of
	$I ->

	    Pid ! stop,

	    C = spawn(fun() -> controller([{sender, ToServer}, {last_telem, void}, {toclient, self()}], initial()) end),
	    
	    [Dx, Dy, TimeLimit, MinSensor, MaxSensor, MaxSpeed, MaxTurn, MaxHardTurn] = read(L),

	    io:format("==== NEW RUN ====~n"),

	    {C};

	$T ->
	    [H10|T10] = L,
	    [TimeStamp] = read([H10]),          % ???
	    [Ctl|T20] = T10,
	    {Telem, _} = read(T20, 4),

	    Pid ! {telem, list_to_tuple([TimeStamp, list_to_tuple(Ctl)] ++ Telem)};

%%%%%%	$E -

	_ -> 
	    Port ! {self(), {command, LL}} 

    end.
x({_, _, X, _, _, _}) -> X.
y({_, _, _, Y, _, _}) -> Y.

dir({_, _, _, _, Dir, _}) -> Dir.


ctl({_, Ctl, _, _, _, _}) -> Ctl.

%%% 


read(L) ->
    lists:map(fun readf/1, L).


readf(X) ->
    case io_lib:fread("~f", X) of 
	{ok, [Y], _} -> 
	    Y; 
	{error, _} -> 
	    {ok, [Y], _} = io_lib:fread("~d", X), 
	    Y 
    end.


read(L, Count) ->
    read(L, Count, []).


read(L, 0, R) ->
    {lists:reverse(R), L};


read([H|T], Count, R) ->
    read(T, Count - 1, [readf(H)|R]).




sender(M) ->
    proplists:get_value(sender, M).

last_telem(M) ->
    proplists:get_value(last_telem, M).

set_last_telem(T, M) ->
    [{last_telem, T}|proplists:delete(last_telem, M)].

%%%


save_telem(M, T) -> 
    set_last_telem(T, M).     



nop() ->
    {fun(_, _) -> 
	     nop()
     end, 
     fun save_telem/2,      
     fun(_M, _) ->
	     void
     end}.



drive1() ->
    {fun(M, _) -> 
	     case last_telem(M) of
		 void ->
		     drive1();
		 _ ->
		     turn_home()
	     end
     end, 
     fun save_telem/2,
     fun(M, _) ->
	     sender(M) ! "a;" 
     end}.


drive() ->
    {fun(_, _) -> 
	     drive()
     end, 
     fun(M, _) -> 
	     M
     end,      
     fun(M, _) ->
	     sender(M) ! ";" 
     end}.
    

compensate() ->
    {fun(_, T) -> 
	     case ctl(T) of
		 {_, $L} ->
		     to_right();
		 {_, $l} ->
		     to_right();
		 
		 {_, $R} ->
		     to_left();
		 {_, $r} ->
		     to_left()
	     end
     end, 

%% not used???
     fun(M, _) -> 
	     M
     end,      
     fun(M, _) ->
	     sender(M) ! ";" 
     end}.

compensate(Angle) ->
    {fun(_, T) -> 
	     case ctl(T) of
		 {_, $L} ->
		     to_right(Angle);
		 {_, $l} ->
		     to_right(Angle);
		 
		 {_, $R} ->
		     to_left(Angle);
		 {_, $r} ->
		     to_left(Angle)
	     end
     end, 

%% not used???
     fun(M, _) -> 
	     M
     end,      
     fun(M, _) ->
	     sender(M) ! ";" 
     end}.





turn_home() ->
    {fun(_, T) ->

	     HH = math:atan(y(T) / x(T)) * 180 / math:pi(),
	     if
		 HH < 0 ->
		     H = 180 - abs(HH);
		 true ->
		     H = HH
	     end,

	     Delta = abs(dir(T) - H),

	     if
		 Delta =< 1 -> 		     
		     io:format("Home!! Ctl=~p.~n", [tuple_to_list(ctl(T))]),
%% ???
		     % turn_home();
		     compensate(H);

		 true ->
 		     turn_home()
	     end
     end,
%     fun save_telem/2,
     fun(M, T) ->
	     set_last_telem(T, M)
     end,

     fun(M, T) ->
%% code duplication	     

	     HH = math:atan(y(T) / x(T)) * 180 / math:pi(),
	     if
		 HH < 0 ->
		     H = 180 - abs(HH);
		 true ->
		     H = HH
	     end,

	     R = dir(T),

%% 	     {_, Turn} = ctl(T),
	     if
		 R > H ->
%% 		     if 
%% 			 (Turn =:= $R) or (Turn =:= $r) ->
%% 			     % sender(M) ! "a;";
%% 			     ok;
%% 			 true ->
			     sender(M) ! "ar;";
%% 		     end;

		 true ->
%% 		     if 
%% 			 (Turn =:= $L) or (Turn =:= $l) ->
%% 			     % sender(M) ! "a;";
%% 			     ok;
%% 			 true ->
			     sender(M) ! "al;"
%% 		     end
	     end
     end}.



%% !!!!!!!
turn_home(Angle) ->
    {fun(_, T) ->

%% 	     HH = Angle,    % ???
%% 	     if
%% 		 HH < 0 ->
%% 		     H = 180 - abs(HH);

%% 		 true ->
%% 		     H = HH
%% 	     end,
	     H = Angle,


	     Delta = abs(dir(T) - H),

	     if
		 Delta =< 1 ->		     
		     io:format("Home!! Ctl=~p.~n", [tuple_to_list(ctl(T))]),
%% ???
		     % turn_home();

		     compensate(H);

		 true ->
 		     turn_home(H)
	     end
     end,
%     fun save_telem/2,
     fun(M, T) ->
	     set_last_telem(T, M)
     end,

     fun(M, T) ->
%% code duplication	     

%% 	     HH = Angle,
%% 	     if
%% 		 HH < 0 ->
%% 		     H = 180 - abs(HH);
%% 		 true ->
%% 		     H = HH
%% 	     end,
	     H = Angle,


	     R = dir(T),

	     io:format("turn_home/1 Dir=~p, Angle=~p.~n", [R, H]),

%% 	     {_, Turn} = ctl(T),
	     if
		 R > H ->
%% 		     if 
%% 			 (Turn =:= $R) or (Turn =:= $r) ->
%% 			     % sender(M) ! "a;";
%% 			     ok;
%% 			 true ->
			     sender(M) ! "ar;";
%% 		     end;

		 true ->
%% 		     if 
%% 			 (Turn =:= $L) or (Turn =:= $l) ->
%% 			     % sender(M) ! "a;";
%% 			     ok;
%% 			 true ->
			     sender(M) ! "al;"
%% 		     end
	     end
     end}.








to_right() ->
    {fun(_, T) ->
	     case ctl(T) of
		 {_, $L} ->
		     to_right();
		 {_, $l} ->
		     to_right();

		 {_, $-} ->
		     io:format("from right --~n"),

		     drive()
	             % F()??? 
	     end
     end,
     fun save_telem/2,
     fun(M, _) ->
	     sender(M) ! "r;"
     end}.

to_left() ->
    {fun(_, T) ->
	     case ctl(T) of
		 {_, $R} ->
		     to_left();
		 {_, $r} ->
		     to_left();

		 {_, $-} ->
		     io:format("from left --~n"),

		     drive()
	             % F()??? 
	     end
     end,
     fun save_telem/2,
     fun(M, _) ->
	     sender(M) ! "l;"
     end}.







%%% taxi up
to_right(Angle) ->
    {fun(_M, T) ->
	     case ctl(T) of
		 {_, $L} ->
		     to_right(Angle);
		 {_, $l} ->
		     to_right(Angle);

		 {_, $-} ->

		     io:format("from right --~n"),

		     R = dir(T),
		     Delta = abs(R - Angle),
		     if 
			 Delta =< 1 ->
			     io:format("SET COURSE Dir=~p, Angle=~p.~n", [R, Angle]),

			     proplists:get_value(toclient, _M) ! don,

			     drive();

			 true ->
			 

			     if
				 R > Angle ->
				     io:format("need to continue to RIGHT~n"),
				     
				     to_right(Angle);

				 true ->
				     io:format("????? need to continue to LEFT to angle"),
			     
				     fun(_, _) -> io:format("!!!!!!!!!~n") end

			     end


		     end;

		 _ -> 

%% ----> alredy turning RIGHT

		     R = dir(T),
		     Delta = abs(R - Angle),
		     if 
			 Delta =< 1 ->
			     to_left(Angle);

			 true ->
			     if
				 R > Angle ->
				     %% check right
				     check_right(Angle);
				     
				 true ->
				     io:format("????? need to continue to LEFT to angle"),
			     
				     fun(_, _) -> io:format("!!!!!!!!!~n") end

			     end
		     end

		    

	     end
     end,
     fun save_telem/2,
     fun(M, _) ->
	     sender(M) ! "r;"
     end}.




check_right(Angle) ->
    { fun(_, Tx) -> Rr = dir(Tx), 
		    Del = abs(Rr - Angle), 
		    if 
			Del =< 1 ->
			    to_left(Angle);
			true ->
			    check_right(Angle)
		    end
      end,
      fun save_telem/2, %% does it matter???
      fun(_M, _) -> void end %%%%%%%% sender(_M) ! ";" end         % or "a;" ????  or simply nothing???
     }.




to_left(Angle) ->
    {fun(_M, T) ->
	     case ctl(T) of
		 {_, $R} ->
		     to_left(Angle);
		 {_, $r} ->
		     to_left(Angle);


%% -------


		 {_, $-} ->

		     io:format("from left --~n"),

		     R = dir(T),
		     Delta = abs(R - Angle),
		     if 
			 Delta =< 1 ->
			     io:format("SET COURSE Dir=~p, Angle=~p.~n", [R, Angle]),
%			     io:format("SET COURSE=~p.~n", [R]),

			     proplists:get_value(toclient, _M) ! don,

			     drive();

			 true ->
			 

			     if
				 R < Angle ->
				     io:format("need to continue to LEFT~n"),
				     
				     to_left(Angle);

				 true ->
				     io:format("????? need to continue to RIGHT to angle"),
			     
				     fun(_, _) -> io:format("!!!!!!!!!~n") end

			     end


		     end;

		 _ -> 

%% ----> alredy turning LEFT

		     R = dir(T),
		     Delta = abs(R - Angle),
		     if 
			 Delta =< 1 ->
			     to_left(Angle);

			 true ->
			     if
				 R < Angle ->
				     %% check right
				     check_left(Angle);
				     
				 true ->
				     io:format("????? need to continue to RIGHT to angle"),
			     
				     fun(_, _) -> io:format("!!!!!!!!!~n") end

			     end
		     end








	     end
     end,
     fun save_telem/2,
     fun(M, _) ->
	     sender(M) ! "l;"
     end}.




check_left(Angle) ->
    { fun(_, Tx) -> Rr = dir(Tx), 
		    Del = abs(Rr - Angle), 
		    if 
			Del =< 1 ->
			    to_right(Angle);
			true ->
			    check_left(Angle)
		    end
      end,
      fun save_telem/2, %% does it matter???
      fun(_M, _) -> void end %%%%%%%% sender(_M) ! ";" end         % or "a;" ????  or simply nothing???
     }.

